/* init.S -- AT91SAM7 rom init code
**
** Copyright 2006, Brian Swetland.  All rights reserved.     
** See provided LICENSE file or http://frotz.net/LICENSE for details.
*/

#include "../library/arm7.h"
		
.globl _start
.globl jump_to_addr
.globl enable_fiq
.globl do_nothing
.globl read_svc_regs
.globl write_svc_regs

#define WITH_DEBUG_SUPPORT 0

#if 1
#define FIQ_TOP_OF_STACK 0x00202000
#define SVC_TOP_OF_STACK 0x00201e00
#else
#define FIQ_TOP_OF_STACK 0x00201000
#define SVC_TOP_OF_STACK 0x00200e00
#endif
#define TOP_OF_RAM       0x00204000

_start:
	b reset_handler
	b .                 /* undefine opcode */
	b .                 /* swi trap */
	b .                 /* prefetch abort */
	b .                 /* data abort */
	b bootloader_start
	b .                 /* irq handler */

fiq_handler:
	/* We save and restore the entire user state (except for CPSR)
	** on the stack to allow for ease of debugging
	*/
	stmfd r13!, {r0-r7, r14}
	ldr r0, =usb_poll
	mov lr, pc
	bx r0
	ldmia r13!, {r0-r7, r14}
	subs pc, lr, #4

/* r0 points to an array of 17 words: R0-R15, CPSR */
read_svc_regs:
	stmfd r13!, {r4-r10}
		
	ldr r10, =(FIQ_TOP_OF_STACK - 9*4)
	ldmia r10, {r1-r9} /* grab R0-R7, PC from the stack */
	stmia r0!, {r1-r8} /* write R0-R7 to the array */

	msr cpsr_c, #(PSR_I | PSR_F | PSR_SVC)
	stmia r0!, {r8-r14}	/* write R8-R14 to the array */
	msr cpsr_c, #(PSR_I | PSR_F | PSR_FIQ)

	mrs r10, spsr
	stmia r0!, {r9, r10} /* write R15, CPSR to the array */
		
	ldmia r13!, {r4-r10}
	bx r14
		
/* r0 points to an array of 17 words */
write_svc_regs:
	bx r14
		
jump_to_addr:
	/*
	** called from within the FIQ handler to cause us to
	** return to a different location when we restore state
	** at the end of the FIQ.
	*/
	ldr r1, =FIQ_TOP_OF_STACK
	add r0, r0, #4
	str r0, [r1, #-4]
	bx lr

reset_handler:
#if WITH_DEBUG_SUPPORT
/* enable clock to the PIO subsystem */
	ldr r1, =0xfffffc00
	mov r0, #4
	str r0, [r1, #0x10]
/* give it a moment to settle */
	mov r0, #64
1:	subs r0, r0, #1
	bne 1b

	ldr r1, =0xfffff400

#if 1
/* Olimex P64 board with a button between PA25 and PA21 */
	mov r0, #(1 << 25)
	str r0, [r1, #0x10]
	str r0, [r1, #0x34]
	ldr r0, [r1, #0x3c]
	tst r0, #(1 << 21)
#else
/* Olimex P64 board with button2 between PA20 and ground */
	ldr r0, [r1, #0x3c]
	tst r0, #(1 << 20)
#endif

#if 0
/* SAMBA shim -> jump back to samba if button held */
	moveq r2, #0x54
	bxeq r2
#else
/* first stage bootloader -> jump to 0x1000 if button *not* held */
	movne r2, #0x1000
	bxne r2
#endif
#endif

	bl init_clock

//	bl init_48mhz_clock

bootloader_start:
/* this entrypoint is used when re-running from ram */

/* init fiq mode stack */
	msr cpsr_c, #(PSR_I | PSR_F | PSR_FIQ)
	ldr sp, =FIQ_TOP_OF_STACK

/* init supervisor mode stack */
	msr cpsr_c, #(PSR_I | PSR_F | PSR_SVC)
	ldr sp, =SVC_TOP_OF_STACK		

/* BUG:	use ldm/stm for much faster copies */
copy_to_ram:
	ldr r0, =0xfffff000
	and r1, pc, r0
	ldr r2, =TEXT_START
	ldr r3, =BSS_START
copy_loop:
	cmp r2, r3
	beq init_bss
	ldr r0, [r1], #4
	str r0, [r2], #4
	b copy_loop

init_bss:
	mov r0, #0
	ldr r1, =BSS_START
	ldr r2, =BSS_END
init_bss_loop:
	cmp r1, r2
	beq wipe_stacks
	str r0, [r1], #4
	b init_bss_loop

wipe_stacks:
	ldr r2, =FIQ_TOP_OF_STACK
	ldr r0, =0xdeadbeef
wipe_loop:
	cmp r1, r2
	beq start
	str r0, [r1], #4
	b wipe_loop

start:
/* enable the NRST reset pin */
	ldr r1, =0xfffffd08
	ldr r0, =0xa5000401
	str r0, [r1]

	ldr r0, =boot
	bx r0

do_nothing:	
	b .

enable_fiq:
	mrs r0, cpsr
	bic r0, r0, #PSR_F
	msr cpsr_c, r0
	bx lr

#define PMC_MCKR   0x30
#define PMC_SR     0x68

#define PMC_MCKRDY     0x08
#define PMC_PRES_DIV2  0x04
#define PMC_CSS_PLL    0x03
		
init_48mhz_clock:
	ldr r1, =0xfffffc00
		
	// turn on /2 prescaler
	mov r0, #PMC_PRES_DIV2
	str r0, [r1, #PMC_MCKR]
wait_for_clock1:
	ldr r0, [r1, #PMC_SR]
	tst r0, #PMC_MCKRDY
	beq wait_for_clock1

	// switch to pll clock
	mov r0, #(PMC_PRES_DIV2 | PMC_CSS_PLL)
	str r0, [r1, #PMC_MCKR]
wait_for_clock2:
	ldr r0, [r1, #PMC_SR]
	tst r0, #PMC_MCKRDY
	beq wait_for_clock2

	mov pc, lr

#define PIN_LED (1 << 8)

#define OUTPUT_ENABLE 0x10
#define DATA_SET 0x30
#define DATA_CLEAR 0x34

#if 0
GREEN:
	ldr r1, =0xfffff400
	mov r0, #PIN_LED
	str r0, [r1, #OUTPUT_ENABLE]
	str r0, [r1, #DATA_CLEAR]
	b .
#endif